#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <sys/mman.h>
#include <stdint.h>

#include <imp/imp_log.h>

#define TAG "regrw"

static int regrw(uint32_t addr, uint32_t *value, int is_write)
{
	void *map_base, *virt_addr;
	off_t target;
	unsigned page_size, mapped_size, offset_in_page;
	int fd, width = 32, ret = 0;

	target = addr;
	fd = open("/dev/mem", is_write ? (O_RDWR | O_SYNC) : (O_RDONLY | O_SYNC));
	if (fd < 0) {
		IMP_LOG_ERR(TAG, "open /dev/mem failed\n");
		return -1;
	}

	mapped_size = page_size = getpagesize();
	offset_in_page = (unsigned)target & (page_size - 1);
	if (offset_in_page + width > page_size) {
		/* This access spans pages.
		 * Must map two pages to make it possible: */
		mapped_size *= 2;
	}
	map_base = mmap(NULL,
					mapped_size,
					is_write ? (PROT_READ | PROT_WRITE) : PROT_READ,
					MAP_SHARED,
					fd,
					target & ~(off_t)(page_size - 1));
	if (map_base == MAP_FAILED) {
		IMP_LOG_ERR(TAG, "mmap failed\n");
		ret = -1;
		goto close_fd;
	}

	virt_addr = (char*)map_base + offset_in_page;

	if (!is_write)
		*value = *(volatile uint32_t*)virt_addr;
	else
		*(volatile uint32_t*)virt_addr = *value;

	if (munmap(map_base, mapped_size) == -1)
		IMP_LOG_ERR(TAG, "munmap failed\n");

close_fd:
	close(fd);

	return ret;
}

#if 0
int read_reg_32(uint32_t addr, uint32_t *value)
{
	return regrw(addr, value, 0);
}

int write_reg_32(uint32_t addr, uint32_t value)
{
	return regrw(addr, &value, 1);
}
#endif
